/**
 * 
 */
import java.awt.*;
import javax.swing.*;
import java.io.*;
import java.util.*;

/**
 * @author superzhb87
 *
 */
public class Robot {
	private JPanel panel;
	private int radius = 5;
	private static int size = 500;
	private double timeOfSize = 1;
	private double STOP = 0, AVOIDSPEED = 0.6, NORMALSPEED = 1.25, ACCELERATEDSPEED = 2.5;//speed constants
	private int SHINEDISTANCE = 50, WEAKDISTANCE = 100, ULTRASOUNDDISTANCE = 20, BARLENGHTH = 5;
//	private final double STOP = 0, AVOIDSPEED = 3, NORMALSPEED = 5, ACCELERATEDSPEED = 10;//speed constants
//	private final int SHINEDISTANCE = 200, WEAKDISTANCE = 800, ULTRASOUNDDISTANCE = 60, BARLENGHTH = 24;
	private final int WANDER = 0, CATCHUP = 1, FOLLOW = 2, AVOID = 3;//These constants are used to represent different current status
	private double speed = NORMALSPEED;
	private double x, y;
	private double direction; //direction is expressed as a angle between the current direction and the X axis
	private int formerDirection;
	private int status = WANDER;
	public static int numberOfRobots = 0;
	public int myNumber;
	private boolean frontClear = true, leftClear = true, rightClear = true;
	private boolean weakIRSensorFront = false, weakIRSensorLeft = false, weakIRSensorRight = false;
	private boolean IRSensorFront[], IRSensorLeft[], IRSensorRight[], IRSensorLeftTotal, IRSensorRightTotal, IRSensorFrontTotal;
	private boolean showIRRange = false, showUSRange = false;
	public static int[][] field;
	public static boolean[][][] infraredOld, infraredNew;
	public static boolean[][] weakInfraredOld, weakInfraredNew;
	private File robotLog;
	public PrintWriter output;
	
	static{
		field = new int[size][size];				//initialize the field, set the boundary to 1
		for(int i = 0; i < size; i++){
			field[0][i] = 1;
			field[size-1][i] = 1;
			field[i][size-1] = 1;
			field[i][0] = 1;
		}
		infraredOld = new boolean[size][size][8];		//initialize the infrared, set all field and frequency clear
		infraredNew = new boolean[size][size][8];
		weakInfraredOld = new boolean[size][size];
		weakInfraredNew = new boolean[size][size];
		for(int i = 0; i < size; i ++)
			for(int j = 0; j < size; j ++){
				weakInfraredOld[i][j] = false;
				weakInfraredNew[i][j] = false;
				for(int k = 0; k < 8; k ++){
					infraredOld[i][j][k] = false;
					infraredNew[i][j][k] = false;
				}
			}
	}
	
	public Robot(JPanel panel, int initX, int initY, double initDirection, boolean demonstrationMode){
		myNumber = numberOfRobots;
		this.panel = panel;
		numberOfRobots ++;
		x = initX;
		y = initY;
		direction = initDirection;
		
		radius *= timeOfSize;
		AVOIDSPEED *= timeOfSize;
		NORMALSPEED *= timeOfSize;
		ACCELERATEDSPEED *= timeOfSize;
		SHINEDISTANCE *= timeOfSize;
		WEAKDISTANCE *= timeOfSize;
		ULTRASOUNDDISTANCE *= timeOfSize;
		BARLENGHTH *= timeOfSize;
		manageSpace(1);
		speed = NORMALSPEED;
		IRSensorLeft = new boolean[8];
		IRSensorRight = new boolean[8];
		IRSensorFront = new boolean[8];
		for(int i = 0; i <= 7; i++){
			IRSensorLeft[i] = false;
			IRSensorRight[i] = false;
			IRSensorFront[i] = false;
		}
		if(demonstrationMode)
			return;
		robotLog = new File("robotLog" + myNumber + ".txt");
		try{
			output= new PrintWriter(new FileWriter(robotLog));
		}
		catch(IOException ex){
			ex.printStackTrace();
		}
	}
	
	
	public void setX(double x){
		this.x = x;
	}
	public void setY(double y){
		this.y = y;
	}
	public void setDirection(double direction){
		this.direction = direction;
	}
	public void setStatus(int status){
		this.status = status;
	}
	
	
	public void nextMove(){
		manageSpace(0);
		switch(status){
		case WANDER:
			wanderNext();
			break;
		case CATCHUP:
			catchupNext();
			break;
		case FOLLOW:
			followNext();
			break;
		case AVOID:
			avoidNext();
			break;
		}
		manageSpace(1);
		
	}
	
	private void manageSpace(int value){
		int manageX, manageY;
		double angle[] = new double[4];
		angle[0] = direction - Math.PI / 6;
		angle[1] = direction + Math.PI / 6;
		angle[2] = direction - Math.PI / 2;
		angle[3] = direction + Math.PI / 2;
		
		//The body of the robots
		for(int i = 0; i < 360; i ++){
			for(float dis = 0; dis <= radius; dis += 0.05){
				manageX = Math.round((float)(x + dis * Math.cos(((float)i / 180) * Math.PI)));
				manageY = Math.round((float)(y + dis * Math.sin(((float)i / 180) * Math.PI)));
				if(manageX < 0 || manageY < 0 || manageX >= size || manageY >= size)
					continue;
				field[manageX][manageY] = value;
			}
		}

		//Effective bones of the robots
		for(int j = 0; j <= 32; j ++){
			for(float i = 0; i <= radius; i += 0.05){
				manageX = Math.round((float)(x + i * Math.cos(direction + j * (Math.PI / 16))));
				manageY = Math.round((float)(y + i * Math.sin(direction + j * (Math.PI / 16))));
				if(manageX < 0 || manageY < 0 || manageX >= size || manageY >= size)
					continue;
				field[manageX][manageY] = value;
			}	
		}
		
		//3 bars of the robots to seperate the signals
		for(int j = 0; j <=3; j++){
			for(float i = 0; i <= radius + BARLENGHTH; i += 0.05){
				manageX = Math.round((float)(x + i * Math.cos(angle[j])));
				manageY = Math.round((float)(y + i * Math.sin(angle[j])));
				if(manageX < 0 || manageY < 0 || manageX >= size || manageY >= size)
					continue;
				field[manageX][manageY] = value;
				manageX = Math.round((float)(x + i * Math.cos(angle[j] + Math.PI / 180)));
				manageY = Math.round((float)(y + i * Math.sin(angle[j] + Math.PI / 180)));
				if(manageX < 0 || manageY < 0 || manageX >= size || manageY >= size)
					continue;
				field[manageX][manageY] = value;
			}
		}

	}
	
/*	private void followNext(){
		speed = NORMALSPEED;
		x += speed * Math.cos(direction);
		y += speed * Math.sin(direction);
		if(checkSector(direction - Math.PI /2, direction + Math.PI /2, x, y, ULTRASOUNDDISTANCE) == false){
			status = AVOID;
			return;
		}
		checkSensors();
		if(IRSensorLeft == -1 && IRSensorFront == -1 && IRSensorRight == -1){
			status = WANDER;
			return;
		}
		
		if((IRSensorFront <=1 || IRSensorFront >= 7) && (IRSensorLeft <=1 || IRSensorLeft >= 7) && (IRSensorRight <=1 || IRSensorRight >= 7)){
			status = WANDER;
			return;
		}
			
		
		if(IRSensorFront != -1 && IRSensorLeft == -1 && IRSensorRight == -1){
			return;
		}
		
		if((IRSensorFront == IRSensorLeft) && (IRSensorFront == IRSensorRight))
			return;
		
		if(IRSensorFront == IRSensorLeft && IRSensorFront != -1){
			direction = direction + 10 * (Math.PI/180);
			return;
		}
		if(IRSensorRight == IRSensorFront && IRSensorFront != -1){
			direction = direction - 10 * (Math.PI/180);
			return;
		}
		
		if(IRSensorLeft != -1){
			direction = direction + 10 * (Math.PI/180);
			return;
		}
		if(IRSensorRight != -1){
			direction = direction - 10 * (Math.PI/180);
			return;
		}
		
		
	}
	*/
	
	private void followNext(){
		int offset = 0;
		speed = NORMALSPEED;
		x += speed * Math.cos(direction);
		y += speed * Math.sin(direction);
		if(checkSector(direction - Math.PI /2, direction + Math.PI /2, x, y, ULTRASOUNDDISTANCE) == false){
			status = AVOID;
			return;
		}
		checkSensors();
		if(!IRSensorLeftTotal && !IRSensorFrontTotal && !IRSensorRightTotal){
			status = WANDER;
			return;
		}
		

		if(IRSensorLeft[2])
			offset -= 1;
		if(IRSensorLeft[3])
			offset -= 1;
		if(IRSensorLeft[4])
			offset -= 15;
		if(IRSensorLeft[5])
			offset -= 17;
		if(IRSensorLeft[6])
			offset -= 20;

		
		if(IRSensorFront[0]){
			double x = 2 * Math.random();
			if(x < 1)
				offset += 20;
			else
				offset -= 20;;
		}
		if(IRSensorFront[1])
			offset -=20;
		if(IRSensorFront[2])
			offset += 15;
		if(IRSensorFront[3])
			offset += 2;
		if(IRSensorFront[5])
			offset -= 2;
		if(IRSensorFront[6])
			offset -= 15;
		if(IRSensorFront[7])
			offset += 20;
		

		if(IRSensorRight[2])
			offset += 20;
		if(IRSensorRight[3])
			offset += 17;
		if(IRSensorRight[4])
			offset += 15;
		if(IRSensorRight[5])
			offset += 1;
		if(IRSensorRight[6])
			offset += 1;
		
		if(IRSensorFront[4])
			offset -= offset * 0.8;
		
		direction += Math.PI * ((double)offset / 180);
		
	}
	private void catchupNext(){
		speed = ACCELERATEDSPEED;
		
		x += speed * Math.cos(direction);
		y += speed * Math.sin(direction);
		
		checkSensors();
		
		if(checkSector(direction - Math.PI /2, direction + Math.PI /2, x, y, ULTRASOUNDDISTANCE) == false){
			status = AVOID;
			return;
		}
		
		if(IRSensorLeftTotal || IRSensorFrontTotal || IRSensorRightTotal){
			status = FOLLOW;
			return;
		}
		
		if(!weakIRSensorLeft && !weakIRSensorRight && !weakIRSensorFront){
			status = WANDER;
			return;
		}
		
		if(weakIRSensorLeft && !weakIRSensorRight)
			direction = direction - 15 * (Math.PI/180);
		if(weakIRSensorRight && !weakIRSensorLeft)
			direction = direction + 15 * (Math.PI/180);
	}
	
	private void wanderNext(){
		speed = NORMALSPEED;
		formerDirection = 0;
		x += speed * Math.cos(direction);
		y += speed * Math.sin(direction);
		if(checkSector(direction - Math.PI /2, direction + Math.PI /2, x, y, ULTRASOUNDDISTANCE) == false){
			status = AVOID;
			return;
		}
		checkSensors();
		if(IRSensorLeftTotal || IRSensorFrontTotal || IRSensorRightTotal){
			status = FOLLOW;
			return;
		}
		if(weakIRSensorLeft || weakIRSensorRight || weakIRSensorFront){
			status = CATCHUP;
			return;
		}

	}
	
	private void avoidNext(){
		frontClear = checkSector(direction - Math.PI/6, direction + Math.PI/6, x, y, ULTRASOUNDDISTANCE);
		rightClear = checkSector(direction + Math.PI/6, direction + Math.PI/2, x, y, ULTRASOUNDDISTANCE);
		leftClear = checkSector(direction - Math.PI/2, direction - Math.PI/6, x, y, ULTRASOUNDDISTANCE);
		
		x += speed * Math.cos(direction);
		y += speed * Math.sin(direction);
		
		if(frontClear&&leftClear&&rightClear){
			status = WANDER;
			return;
		}
		
		if(frontClear&&leftClear&&!rightClear){
			direction = direction - 15 * (Math.PI/180);
			speed = AVOIDSPEED;
			return;
		}
		
		if(frontClear&&!leftClear&&rightClear){
			direction = direction + 15 * (Math.PI/180);
			speed = AVOIDSPEED;
			return;
		}
		
/*		if(frontClear&&!leftClear&&!rightClear){
			speed = AVOIDSPEED;
			return;
		}
*/		
		if(!frontClear&&leftClear&&rightClear){
			if(formerDirection == 0){
				double x = 2 * Math.random();
				if(x < 1)
					formerDirection = -1;
				else
					formerDirection = 1;
			}
			
			direction = direction + formerDirection * 15 * (Math.PI/180);
			
			speed = AVOIDSPEED;
			return;
		}
		
		if(!frontClear&&leftClear&&!rightClear){
			direction = direction - 15 * (Math.PI/180);
			speed = AVOIDSPEED;
			return;
		}
		
		if(!frontClear&&!leftClear&&rightClear){
			direction = direction + 15 * (Math.PI/180);
			speed = AVOIDSPEED;
			return;
		}
		
		if(!leftClear&&!rightClear){
			if(formerDirection == 0){
				double x = 2 * Math.random();
				if(x < 1)
					formerDirection = -1;
				else
					formerDirection = 1;
			}
			
			speed = STOP;
			direction = direction + formerDirection * 15 * (Math.PI/180);
			return;
		}
	}
	
	private boolean checkSector(double theta1, double theta2, double x, double y, int distance){
		int checkX, checkY;
		boolean isClear = true;
		double theta = theta1;
outer:		while(theta < theta2){
				for(int dis = radius; dis <= distance; dis++){
					checkX = Math.round((float)(x + Math.cos(theta) * dis));
					checkY = Math.round((float)(y + Math.sin(theta) * dis));
					if(checkX < 0 || checkY < 0 || checkX >= size || checkY >= size)
						continue;
					if(field[checkX][checkY] == 1){
						isClear = false;
						break outer;
					}
			
				}
				theta = theta + Math.PI /360;
			}
		return isClear;
	}
	
	
	public void draw(){
		double angle[] = new double[4];
		angle[0] = direction - Math.PI / 6;
		angle[1] = direction + Math.PI / 6;
		angle[2] = direction - Math.PI / 2;
		angle[3] = direction + Math.PI / 2;
		
		int roundedX = Math.round((float)x);
		int roundedY = Math.round((float)y);
		Graphics paper = panel.getGraphics();
		switch(status){
		case WANDER:
			paper.setColor(Color.GREEN);
			break;
		case FOLLOW:
			paper.setColor(Color.BLUE);
			break;
		case AVOID:
			paper.setColor(Color.RED);
			break;
		case CATCHUP:
			paper.setColor(Color.orange);
			break;
		}

		paper.fillOval(roundedX - radius, roundedY - radius, 2*radius, 2*radius);
		if(showUSRange){
			paper.setColor(Color.BLUE);
			paper.drawOval(roundedX - ULTRASOUNDDISTANCE, roundedY - ULTRASOUNDDISTANCE, 2 * ULTRASOUNDDISTANCE, 2 * ULTRASOUNDDISTANCE);
		}
		if(showIRRange){
			paper.setColor(Color.red);
			paper.drawOval(roundedX - SHINEDISTANCE, roundedY - SHINEDISTANCE, 2*SHINEDISTANCE, 2*SHINEDISTANCE);
		}
//		paper.setColor(Color.GRAY);
//		paper.drawOval(roundedX - WEAKDISTANCE, roundedY - WEAKDISTANCE, 2 * WEAKDISTANCE, 2 * WEAKDISTANCE);
		paper.setColor(Color.BLACK);
		paper.drawLine(roundedX, roundedY, roundedX + (int)((radius - 1) * Math.cos(direction)), roundedY + (int)((radius - 1) * Math.sin(direction)));
		for(int i = 0; i <=3; i ++)
			paper.drawLine((int)(x + radius * Math.cos(angle[i])), (int)(y + radius * Math.sin(angle[i])), (int)(x + (radius + BARLENGHTH) * Math.cos(angle[i])), (int)(y + (radius + BARLENGHTH) * Math.sin(angle[i])));
	}
	
	public void delete(){
		double angle[] = new double[4];
		angle[0] = direction - Math.PI / 6;
		angle[1] = direction + Math.PI / 6;
		angle[2] = direction - Math.PI / 2;
		angle[3] = direction + Math.PI / 2;
		int roundedX = Math.round((float)x);
		int roundedY = Math.round((float)y);
		Graphics paper = panel.getGraphics();
		paper.setColor(Color.WHITE);
		
		paper.fillOval(roundedX - radius, roundedY - radius, 2 * radius, 2 * radius);	
//		paper.fillOval(roundedX - SHINEDISTANCE - 5, roundedY - SHINEDISTANCE - 5, 2 * SHINEDISTANCE + 10, 2 * SHINEDISTANCE + 10);
		if(showUSRange)
			paper.drawOval(roundedX - ULTRASOUNDDISTANCE, roundedY - ULTRASOUNDDISTANCE, 2 * ULTRASOUNDDISTANCE, 2 * ULTRASOUNDDISTANCE);
		if(showIRRange)
			paper.drawOval(roundedX - SHINEDISTANCE, roundedY - SHINEDISTANCE, 2*SHINEDISTANCE, 2*SHINEDISTANCE);
//		paper.drawOval(roundedX - WEAKDISTANCE, roundedY - WEAKDISTANCE, 2 * WEAKDISTANCE, 2 * WEAKDISTANCE);
		for(int i = 0; i <=3; i ++)
			paper.drawLine((int)(x + radius * Math.cos(angle[i])), (int)(y + radius * Math.sin(angle[i])), (int)(x + (radius + BARLENGHTH) * Math.cos(angle[i])), (int)(y + (radius + BARLENGHTH) * Math.sin(angle[i])));

	}
	
	public void shine(){
		double theta1, theta2;
		
		for(int i = 0; i <= 7; i ++){
			theta1 = direction + i * (Math.PI / 4) - Math.PI / 10;
			theta2 = direction + i * (Math.PI / 4) + Math.PI / 10;
			shineSector(theta1, theta2, SHINEDISTANCE, i);
		}
	}

	
	private void shineSector(double theta1, double theta2, int distance, int direction){ //here direction means one of the eight directions on the robots
		int roundedX, roundedY;
		double theta = theta1;
		while(theta < theta2){
			for(int i = radius + BARLENGHTH + 1; i <= distance; i ++){
				roundedX = Math.round((float)(x + i * Math.cos(theta)));
				roundedY = Math.round((float)(y + i * Math.sin(theta)));
				if(roundedX < 0 || roundedX >= size || roundedY < 0 || roundedY >= size)
					break;
				if(field[roundedX][roundedY] == 1)
					break;
				infraredNew[roundedX][roundedY][direction] = true;
			}
			for(int i = distance + 1; i <= WEAKDISTANCE; i ++){
				roundedX = Math.round((float)(x + i * Math.cos(theta)));
				roundedY = Math.round((float)(y + i * Math.sin(theta)));
				if(roundedX < 0 || roundedX >= size || roundedY < 0 || roundedY >= size)
					break;
				if(field[roundedX][roundedY] == 1)
					break;
				weakInfraredNew[roundedX][roundedY] = true;
			}

			theta = theta + Math.PI /720;
		}
		
	}
	
	private void checkSensors(){
		for(int i = 0; i <= 7; i ++){
			IRSensorFront[i] = false;
			IRSensorRight[i] = false;
			IRSensorLeft[i] = false;
		}
		IRSensorLeftTotal = false;
		IRSensorRightTotal = false;
		IRSensorFrontTotal = false;
		weakIRSensorFront = false;
		weakIRSensorLeft = false;
		weakIRSensorRight = false;
		
		int roundedX, roundedY;
		double theta;

out:	for(int i = 0; i <= 7; i++){
			theta = direction + Math.PI / 3 - Math.PI / 8;
in:			while(theta < direction + Math.PI / 3 + Math.PI / 8){

				roundedX = Math.round((float)(x + (radius) * Math.cos(theta)));
				roundedY = Math.round((float)(y + (radius) * Math.sin(theta)));
				if(weakInfraredOld[roundedX][roundedY] && weakIRSensorRight == false){
					weakIRSensorRight = true;
				}
				if(infraredOld[roundedX][roundedY][i]){
					IRSensorRight[i] = true;
					IRSensorRightTotal = true;
					break out;
				}
				theta = theta + Math.PI /360;
			}
		}
		
		
out:	for(int i = 0; i <= 7; i ++){
			theta = direction - Math.PI / 8;
in:			while(theta < direction + Math.PI / 8){

				roundedX = Math.round((float)(x + (radius) * Math.cos(theta)));
				roundedY = Math.round((float)(y + (radius) * Math.sin(theta)));
				if(weakInfraredOld[roundedX][roundedY] && weakIRSensorFront == false){
					weakIRSensorFront = true;
				}
				if(infraredOld[roundedX][roundedY][i]){
					IRSensorFront[i] = true;
					IRSensorFrontTotal = true;
					break out;	
				}
				theta = theta + Math.PI /360;
			}
		}



out:	for(int i = 0; i <= 7; i ++){
			theta = direction - Math.PI / 3 - Math.PI / 8;
in:			while(theta < direction - Math.PI / 3 + Math.PI / 8){

				roundedX = Math.round((float)(x + (radius) * Math.cos(theta)));
				roundedY = Math.round((float)(y + (radius) * Math.sin(theta)));
				if(weakInfraredOld[roundedX][roundedY] && weakIRSensorLeft == false){
					weakIRSensorLeft = true;
				}
				if(infraredOld[roundedX][roundedY][i]){
					IRSensorLeft[i] = true;
					IRSensorLeftTotal = true;
					break out;	
				}
				theta = theta + Math.PI /360;
			}
		}		
	}
	
	
	public void report(){
		output.print(x + "\t");
		output.print(y + "\t");
		output.print(direction + "\t");
		output.println(status);
	}
}
